from CBS_package import high_level
from CBS_package import low_level
import numpy as np
import matplotlib as plt 
import random
import visualizer
import ruamel_yaml as yaml
# from ruamel import yaml
import os

def practical(result, goal):
    for i,point in enumerate(result):
        goal = list(goal)
        point = list(point)
        if point == goal:
            return i


def compare(result, goals):
    manhattan = (np.linalg.norm(result[0][-1] - result[0][0],1) + np.linalg.norm(result[1][-1] - result[1][0],1))/80
    Practical = practical(result[0],goals[0]) + practical(result[1],goals[1])

    if manhattan == Practical:
        return 0
    else:
        return 1

def transport_int(L):
    new_list = [[0,0],[0,0]]
    for j in range(2):
        for i in range(2):
            a = int(L[j][i])
            new_list[j][i] = a
        new_list[j] = tuple(new_list[j])
    return new_list
# static_obstacles = [(0, 0), (1500, 1500)]


results = []

matrix = np.zeros((10,10), dtype = [('x', 'i4'), ('y', 'i4')])
# numberList = list(range(40, 840, 80))

for row in range(10):
    for collumn in range(10):
        matrix[collumn][row] = (row*80+40, collumn*80+40)

i = 1
manhattan_path = 0

while i <= 10:
    starts = [i for i in (np.random.choice( matrix.ravel(),2,replace=False))]
    goals = [i for i in (np.random.choice( matrix.ravel(),2,replace=False))]

    starts = transport_int(starts)
    goals = transport_int(goals)
    
    with open('scenario.yml') as fd: 
        content = yaml.load(fd, Loader=yaml.RoundTripLoader)
        content.update({'GOAL': goals})
        content.update({'START': starts})

    with open('scenario.yml', 'w') as nf:
        yaml.dump(content, nf, Dumper=yaml.RoundTripDumper)

    visualizer.load_scenario('scenario.yml')
    simulator = visualizer.Simulator()

    if simulator.path:
        unoptimizable = compare(simulator.path, goals)
    else:
        print('starts:', starts)
        print('    goals:/n', goals)
    if unoptimizable:
        simulator.start(i)
        i = i+1
    else:
        manhattan_path += 1
    
    print('manhattan_path:', manhattan_path)



